import cv2
import numpy as np

def calculate_straightness(points):
    # 确保 points 是 numpy 数组并且数据类型为 float32
    points = np.array(points, dtype=np.float32)
    
    # 第一个角点和最后一个角点
    point1 = points[0]
    point2 = points[-1]
    
    # 计算这两个点之间的距离
    dist = np.linalg.norm(point2 - point1)
    
    # 计算以这两个点为边长的等边三角形的第三点
    midpoint = (point1 + point2) / 2
    height = dist * np.sin(np.radians(60))
    direction = (point2 - point1) / dist
    perpendicular_direction = np.array([-direction[1], direction[0]])
    third_point = midpoint + height * perpendicular_direction
    
    # 构建等边三角形的顶点
    equilateral_triangle = np.array([point1, point2, third_point], dtype=np.float32)
    
    # 计算等边三角形的面积
    area_equilateral = cv2.contourArea(equilateral_triangle)
    
    # 计算前num_points个点形成的多边形的面积
    polygon_area = cv2.contourArea(points)
    
    # 计算直线度
    straightness = (area_equilateral - polygon_area) / area_equilateral
    
    return straightness

def calculate_parallelism(line1, line2):
    """
    Calculate the parallelism between two lines.
    Parallelism is defined as cos(theta), where theta is the angle between the two lines.
    If theta is 0 degrees, parallelism is 1.
    If theta is 90 degrees, parallelism is 0.
    """
    # Ensure line1 and line2 are numpy arrays with dtype float32
    line1 = np.array(line1, dtype=np.float32)
    line2 = np.array(line2, dtype=np.float32)
    
    # Get the direction vectors of the lines
    dir_vector1 = line1[-1] - line1[0]
    dir_vector2 = line2[-1] - line2[0]
    
    # Normalize the direction vectors
    dir_vector1 /= np.linalg.norm(dir_vector1)
    dir_vector2 /= np.linalg.norm(dir_vector2)
    
    # Calculate the dot product of the direction vectors
    dot_product = np.dot(dir_vector1, dir_vector2)
    
    # The parallelism is the absolute value of the dot product
    parallelism = abs(dot_product)
    parallelism = (parallelism-0.9)*10
    
    return parallelism

import numpy as np


def detect_corners(image_path, pattern_size=(6, 9)):
    image = cv2.imread(image_path)
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    
    if ret:
        # Extract corners and organize into a matrix
        corners = corners.reshape(pattern_size[1], pattern_size[0], 2)
        
        all_straightness_values = []
        all_parallelism_long_lines = []
        all_parallelism_short_lines = []



        # Calculate straightness for each long line
        for i in range(pattern_size[1]):
            long_line_points = corners[i, :]
            if i == 0:
                first_short_line = long_line_points
            elif i == pattern_size[1] - 1:
                last_short_line = long_line_points
                parallelism_short_lines = calculate_parallelism(first_short_line, last_short_line)
                print(f"parallelism_short_lines = {parallelism_short_lines}")
            straightness = calculate_straightness(long_line_points)
            all_straightness_values.append(straightness)
            
            # Calculate parallelism for adjacent long lines
            if i < pattern_size[1] - 1:
                next_long_line_points = corners[i+1, :]
                parallelism = calculate_parallelism(long_line_points, next_long_line_points)
                all_parallelism_long_lines.append(parallelism)
        
        # Calculate straightness for each short line
        for j in range(pattern_size[0]):
            short_line_points = corners[:, j]
            if j == 0:
                first_long_line = short_line_points
            elif j == pattern_size[0] - 1:
                last_long_line = short_line_points
                parallelism_long_lines = calculate_parallelism(first_long_line, last_long_line)
                print(f"parallelism_long_lines = {parallelism_long_lines}")
            straightness = calculate_straightness(short_line_points)
            all_straightness_values.append(straightness)
            
            # Calculate parallelism for adjacent short lines
            if j < pattern_size[0] - 1:
                next_short_line_points = corners[:, j+1]
                parallelism = calculate_parallelism(short_line_points, next_short_line_points)
                all_parallelism_short_lines.append(parallelism)
        
        # Statistics for straightness
        max_straightness = max(all_straightness_values)
        min_straightness = min(all_straightness_values)
        avg_straightness = sum(all_straightness_values) / len(all_straightness_values)
        
        # Statistics for parallelism of long lines
        max_parallelism_long_lines = max(all_parallelism_long_lines) if all_parallelism_long_lines else None
        min_parallelism_long_lines = min(all_parallelism_long_lines) if all_parallelism_long_lines else None
        avg_parallelism_long_lines = sum(all_parallelism_long_lines) / len(all_parallelism_long_lines) if all_parallelism_long_lines else None
        
        # Statistics for parallelism of short lines
        max_parallelism_short_lines = max(all_parallelism_short_lines) if all_parallelism_short_lines else None
        min_parallelism_short_lines = min(all_parallelism_short_lines) if all_parallelism_short_lines else None
        avg_parallelism_short_lines = sum(all_parallelism_short_lines) / len(all_parallelism_short_lines) if all_parallelism_short_lines else None
        
        # Print results
        print(f"Max Straightness: {max_straightness}")
        print(f"Min Straightness: {min_straightness}")
        print(f"Avg Straightness: {avg_straightness}")
        
        if all_parallelism_long_lines:
            print(f"Max Parallelism Long Lines: {max_parallelism_long_lines}")
            print(f"Min Parallelism Long Lines: {min_parallelism_long_lines}")
            print(f"Avg Parallelism Long Lines: {avg_parallelism_long_lines}")
        
        if all_parallelism_short_lines:
            print(f"Max Parallelism Short Lines: {max_parallelism_short_lines}")
            print(f"Min Parallelism Short Lines: {min_parallelism_short_lines}")
            print(f"Avg Parallelism Short Lines: {avg_parallelism_short_lines}")
    else:
        print("Chessboard corners not found.")

# Example call
detect_corners('images_processing/img_out/undistorted_img.jpg')